#include "WiFi.h"
#include "AsyncUDP.h"
#include "SWD.h"
#include "STLinkV2.h"
const char * ssid = "D1-402";
const char * password = "lbsvip168";

u8 databuff[6144+1024];
AsyncUDP udp;

#define     LED_D4     12
#define     LED_D5     13

// class AsyncUDPPacket lastpacket;

u32 rpos;
u8 *wp;
u32 wlen;
u32 rd(u8* p,u32 len){
  while(len--)
    *p++=databuff[rpos++];
  return len;
}
u32 wd(u8* p,u32 len){
  wp=p;
  wlen=len;
  return len;
}
const char *defstr[256];
void initstr()
{
  int i;
  for (i = 0; i < 256; i++)
    defstr[i] = "undef";
  defstr[0x01] = "GETSTATUS";
  defstr[0x02] = "FORCEDEBUG";
  defstr[0x03] = "APIV1_RESETSYS";
  defstr[0x04] = "APIV1_READALLREGS";
  defstr[0x05] = "APIV1_READREG";
  defstr[0x06] = "APIV1_WRITEREG";
  defstr[0x07] = "READMEM_32BIT";
  defstr[0x08] = "WRITEMEM_32BIT";
  defstr[0x09] = "RUNCORE";
  defstr[0x0a] = "STEPCORE";
  defstr[0x0b] = "APIV1_SETFP";
  defstr[0x0c] = "READMEM_8BIT";
  defstr[0x0d] = "WRITEMEM_8BIT";
  defstr[0x0e] = "APIV1_CLEARFP";
  defstr[0x0f] = "APIV1_WRITEDEBUGREG";
  defstr[0x10] = "APIV1_SETWATCHPOINT";
  defstr[0x00] = "ENTER_JTAG_RESET";
  defstr[0xa3] = "ENTER_SWD_NO_RESET";
  defstr[0xa4] = "ENTER_JTAG_NO_RESET";
  defstr[0x20] = "APIV1_ENTER";
  defstr[0x21] = "EXIT";
  defstr[0x22] = "READCOREID";
  defstr[0x30] = "APIV2_ENTER";
  defstr[0x31] = "APIV2_READ_IDCODES";
  defstr[0x32] = "APIV2_RESETSYS";
  defstr[0x33] = "APIV2_READREG";
  defstr[0x34] = "APIV2_WRITEREG";
  defstr[0x35] = "APIV2_WRITEDEBUGREG";
  defstr[0x36] = "APIV2_READDEBUGREG";
  defstr[0x3A] = "APIV2_READALLREGS";
  defstr[0x3B] = "APIV2_GETLASTRWSTATUS";
  defstr[0x3C] = "APIV2_DRIVE_NRST";
  defstr[0x3E] = "APIV2_GETLASTRWSTATUS2";
  defstr[0x40] = "APIV2_START_TRACE_RX";
  defstr[0x41] = "APIV2_STOP_TRACE_RX";
  defstr[0x42] = "APIV2_GET_TRACE_NB";
  defstr[0x43] = "APIV2_SWD_SET_FREQ";
  defstr[0x44] = "APIV2_JTAG_SET_FREQ";
  defstr[0x45] = "APIV2_READ_DAP_REG";
  defstr[0x46] = "APIV2_WRITE_DAP_REG";
  defstr[0x47] = "APIV2_READMEM_16BIT";
  defstr[0x48] = "APIV2_WRITEMEM_16BIT";
  defstr[0x4B] = "APIV2_INIT_AP";
  defstr[0x4C] = "APIV2_CLOSE_AP_DBG";
  defstr[0x50] = "WRITEMEM_32BIT_NO_ADDR_IN";
  defstr[0x51] = "APIV2_RW_MISC_OUT";
  defstr[0x52] = "APIV2_RW_MISC_IN";
  defstr[0x54] = "READMEM_32BIT_NO_ADDR_INC";
  defstr[0x61] = "SET_COM_FREQ";
  defstr[0x62] = "GET_COM_FREQ";
  defstr[0xF1] = "GET_VERSION";
  defstr[0xF2] = "DEBUG_COMMAND";
  defstr[0xF3] = "DFU_COMMAND";
  defstr[0xF5] = "GET_CURRENT_MODE";
  defstr[0xF7] = "GET_TARGET_VOLTAGE";
  defstr[0xFB] = "GET_VERSION_APIV3";
}
u32 r4(u8 **i)
{
	u32 v = 0;
	u8 *p = *i;
	_32T8LL(v) = *p++;
	_32T8L(v) = *p++;
	_32T8H(v) = *p++;
	_32T8HH(v) = *p++;
	*i = p;
	return v;
}

u32 r2(u8 **i)
{
	u32 v = 0;
	u8 *p = *i;
	_16T8L(v) = *p++;
	_16T8H(v) = *p++;
	*i = p;
	return v;
}
void setup() {
  initstr();
  Serial.begin(115200);
  pinMode(LED_D4, OUTPUT);
  pinMode(LED_D5, OUTPUT);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  STLinkV2.Init(databuff);
  if(SWD.Connect()){
    Serial.printf("ID:%08x\r\n",SWD.IDCores());
    SWD.Setup();
    SWD.Halt();
    SWD.SetDataLenght(2);
    u32 regvalue;
    u32 value[2];
    value[0]=value[1]=0;
    SWD.WriteMem(0x20000000,8,(u32*)&value);
    SWD.SetDataLenght(0);
    value[0]=0x12345678;
    value[1]=0x90ABCDEF;
    SWD.WriteMem(0x20000001,6,(u32*)&value);
    SWD.SetDataLenght(2);
    value[0]=value[1]=0;
    SWD.ReadMem(0x20000000,8,(u32*)&value);
    Serial.printf("value:%08x %08x\r\n",value[0],value[1]);
  }
  if (WiFi.waitForConnectResult() != WL_CONNECTED) {
        Serial.println("WiFi Failed");
        while(1) {
            delay(1000);
        }
  }
  if(udp.listen(51)) {
        udp.onPacket([](AsyncUDPPacket packet) {
            //lastpacket=packet;
            u16 len = packet.read(databuff + 1024, 6144);
            u8 *p;
            if (len < 5)
            {
              if (databuff[1024] == 't' && databuff[1025] == 'e')
              {
                ((u32 *)databuff)[0] = 0xffffffff;
                packet.write(databuff, 4);
              }
              return;
            }
            // p=databuff+1024+6;
            // Serial.printf("[%02x]%s \r\n",(u32)databuff[1024+5],defstr[databuff[1024+5]]);
            // u32 paddr=r4(&p);
            // u32 plen=r4(&p);
            // p=databuff+1024+databuff[1024]+4;
            // u32 pdt=r4(&p);
            // Serial.printf("%08X %08X %08X\r\n",paddr,plen,pdt);
            rpos = 1024;
            wlen=0;
            STLinkV2.CMD(rd, wd);
            // Serial.printf("write %d %08X %08X\r\n",wlen,((u32*)wp)[0],((u32*)wp)[1]);
            if (wlen)
              packet.write(wp, wlen);
            //packet.printf("Got %u bytes of data", packet.length());
        });
    }
}


void loop() {
  // u8 sta=SWD.Connect();
  // Serial.printf("swd:%s\r\n",sta?"true":"false");
  // if(sta){
  //   Serial.printf("ID:%08x\r\n",SWD.IDCores());
  // }
  // else{

  // digitalWrite(LED_D4, LOW);
  // digitalWrite(LED_D5, HIGH);
  // delay(500);
  // digitalWrite(LED_D4, HIGH);
  // digitalWrite(LED_D5, LOW);
  // delay(500);
  // //Send broadcast
  // udp.broadcast("Anyone here?");
  // }
}
